#ifndef __BALANCE_H
#define __BALANCE_H			  	 
#include "sys.h"
#include "system.h"

#define BALANCE_TASK_PRIO		5     //Task priority //任务优先级
#define BALANCE_STK_SIZE 		1024   //Task stack size //任务堆栈大小

extern xTaskHandle TestHandle;

//Parameter of kinematics analysis of omnidirectional trolley
//全向轮小车运动学分析参数
#define X_PARAMETER    (sqrt(3)/2.f)               
#define Y_PARAMETER    (0.5f)    
#define L_PARAMETER    (1.0f)   

extern int A,B,C,DD;
void Balance_task(void *pvParameters);
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d);
void Limit_Pwm(int amplitude);
float target_limit_float(float insert,float low,float high);
int target_limit_int(int insert,int low,int high);
u8 Turn_Off( int voltage);
u32 myabs(long int a);
int Incremental_PI_A (float Encoder,float Target);
int Incremental_PI_B (float Encoder,float Target);
int Incremental_PI_C (float Encoder,float Target);
int Incremental_PI_D (float Encoder,float Target);
float Vz_Control(void); //float Angle_Z,float Target_AngleZ);
void S_Control(float *Vx,float *Vy);


//void Get_RC(void);
//void Remote_Control(void);
void Drive_Motor(float Vx,float Vy,float Vz);
void Key(void);
void Get_Velocity_Form_Encoder(void);
void Smooth_control(float vx,float vy,float vz);
//void Smooth_control_ROS(float vx,float vy,float vz);
//void PS2_control(void);
float float_abs(float insert);
//void robot_mode_check(void);

#endif  

